#include "DemoWidget.h"
#include <Poco/Thread.h>

#include <pcl/io/pcd_io.h>
#include <pcl/compression/compression_profiles.h>
#include <pcl/visualization/common/float_image_utils.h> //保存深度图像
#include <pcl/io/png_io.h>                              //保存深度图像
#include <pcl/console/print.h>

#include <Eigen/Dense>

#include "PointCloudHandler.h"
#include "ImageHandler.h"
#include "Utils.h"

#pragma region Init
DemoWidget::DemoWidget(QWidget *parent) : QWidget(parent)
{
    setupUi();
    // InitTCP();

    // TestReadStream();
    // return;
    //  TestImage();

    // 初始化图像
    // image = cv::imread("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/test.png");
    // cv::resize(image, image, cv::Size(4096, 600));
    // image_write = cv::imread("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/test.png");

    // 初始化可视化界面
    pcl_viewer = boost::make_shared<pcl::visualization::PCLVisualizer>("3D Viewer", false);
    pcl_viewer->setBackgroundColor(1, 1, 1);
    pcl_viewer->addCoordinateSystem(1.0);
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl_viewer->addPointCloud<pcl::PointXYZI>(pcl_pointcloud, "pointcloud");
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_keypoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl_viewer->addPointCloud<pcl::PointXYZ>(pcl_keypoints, "keypoints");

    vtkWidget->SetRenderWindow(pcl_viewer->getRenderWindow());
    pcl_viewer->setupInteractor(vtkWidget->GetInteractor(), vtkWidget->GetRenderWindow());
    pcl_viewer->getInteractorStyle()->setKeyboardModifier(pcl::visualization::INTERACTOR_KB_MOD_SHIFT);

    pcl::visualization::Camera camera_params;
    pcl_viewer->getCameraParameters(camera_params);
    camera_params.pos[0] = 9765.14;
    camera_params.pos[1] = -34245.6;
    camera_params.pos[2] = 2990.44;
    camera_params.view[0] = -0.0192405;
    camera_params.view[1] = 0.08153;
    camera_params.view[2] = 0.996485;
    pcl_viewer->setCameraParameters(camera_params);

    pcl_viewer_range = std::make_shared<pcl::visualization::RangeImageVisualizer>("Range Image");

    timer = new QTimer(this);
    // connect(timer, &QTimer::timeout, this, &DemoWidget::updateSavePCD);
    connect(timer, &QTimer::timeout, this, &DemoWidget::updateViewer);
    // connect(timer, &QTimer::timeout, this, &DemoWidget::updateSaveStream);
    // connect(timer, &QTimer::timeout, this, &DemoWidget::updateSendStream);
    timer->start(100);

    // 启动lidar
    _dataManager = LidarDataManager::getInstance();
    _option = new LidarConfigurationOptions();
    _option->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
    _option->SetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort, (uint16_t)6699);
    _option->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)7788);
    _option->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::RS_M1);
    _option->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::DENSE);
    _option->SetValue(LidarConfigurationOptions::Data_Decode_SampleVoxel, (float)0.05f);
    //_dataManager->UseOption(_option);
 
    std::vector<LidarConfigurationOptions *> _options;
    LidarConfigurationOptions *_option1 = new LidarConfigurationOptions();
    _option1->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort, (uint16_t)6699);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)7788);
    _option->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::RS_M1);
    // TransformParam _transform1;
    // _transform1.yaw = M_PI / 2;
    // _option1->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform1);
    _options.push_back(_option1);
    // LidarConfigurationOptions *_option2 = new LidarConfigurationOptions();
    // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort, (uint16_t)9966);
    // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)8877);
    // _options.push_back(_option2);
    //_dataManager->UseOptions(_options);

    // 端口数据
    {
        // LidarConfigurationOptions *_option1 = new LidarConfigurationOptions();
        // _option1->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
        // _option1->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::DENSE);
        // _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_RemoteIP, std::string(""));
        // _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_LocalIP, std::string(""));
        // _option1->SetValue(LidarConfigurationOptions::Data_Decode_CorrectionFile, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/files/1.dat"));
        // _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)2361);
        // _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::AT_128);
        // TransformParam _transform1;
        // _transform1.y = 3500;
        // _transform1.x = 470;
        // _transform1.yaw = M_PI * 2;
        // _option1->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform1);
        // _options.push_back(_option1);

        // LidarConfigurationOptions *_option2 = new LidarConfigurationOptions();
        // _option2->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
        // _option2->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::DENSE);
        // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_RemoteIP, std::string(""));
        // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_LocalIP, std::string(""));
        // _option2->SetValue(LidarConfigurationOptions::Data_Decode_CorrectionFile, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/files/2.dat"));
        // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)2362);
        // _option2->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::AT_128);
        // TransformParam _transform2;
        // _transform2.yaw = M_PI;
        // _transform2.x = -470;
        // _transform2.y = 3500;
        // _option2->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform2);
        // _options.push_back(_option2);

        // LidarConfigurationOptions *_option3 = new LidarConfigurationOptions();
        // _option3->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
        // _option3->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::DENSE);
        // _option3->SetValue(LidarConfigurationOptions::Data_SrcNet_RemoteIP, std::string(""));
        // _option3->SetValue(LidarConfigurationOptions::Data_SrcNet_LocalIP, std::string(""));
        // _option3->SetValue(LidarConfigurationOptions::Data_Decode_CorrectionFile, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/files/3.dat"));
        // _option3->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)2363);
        // _option3->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::AT_128);
        // TransformParam _transform3;
        // _transform3.y = -800;
        // _transform3.x = -100;
        // _transform3.yaw = M_PI + M_PI / 40.0;
        // _option3->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform3);
        // _options.push_back(_option3);
    }

    // PCD文件
    {
        // LidarConfigurationOptions *_option1 = new LidarConfigurationOptions();
        // _option1->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::PCD_FILE);
        // //_option1->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::FILTERED);
        // _option1->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/dalian/2361.pcd"));
        // //_option1->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/20250415132128551.pcd"));
        // TransformParam _transform1;
        // _transform1.x = 470;
        // _transform1.y = 3500;
        // _transform1.yaw = M_PI * 2;
        // _option1->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform1);
        // _options.push_back(_option1);

        // LidarConfigurationOptions *_option2 = new LidarConfigurationOptions();
        // _option2->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::PCD_FILE);
        // //_option2->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::FILTERED);
        // _option2->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/dalian/2362.pcd"));
        // TransformParam _transform2;
        // _transform2.yaw = M_PI;
        // _transform2.x = -470;
        // _transform2.y = 3500;
        // _option2->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform2);
        // _options.push_back(_option2);

        // LidarConfigurationOptions *_option3 = new LidarConfigurationOptions();
        // _option3->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::PCD_FILE);
        // //_option3->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::FILTERED);
        // _option3->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/dalian/2363.pcd"));
        // TransformParam _transform3;
        // _transform3.x = -100;
        // _transform3.y = -800;
        // _transform3.yaw = M_PI + M_PI / 40.0;
        // _option3->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform3);
        // _options.push_back(_option3);

        // // LidarConfigurationOptions *_option4 = new LidarConfigurationOptions();
        // // _option4->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::PCD_FILE);
        // // _option4->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/dalian/2364.pcd"));
        // // TransformParam _transform4;
        // // _transform4.y = -800;
        // // _transform4.x = 900;
        // // _transform4.roll = -M_PI / 40.0;
        // // _option4->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform4);
        // // _options.push_back(_option4);
    }

    // {
    //     LidarConfigurationOptions *_option2 = new LidarConfigurationOptions();
    //     _option2->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::PCD_FILE);
    //     //_option2->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::FILTERED);
    //     _option2->SetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath, std::string("/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/20250421155318638.pcd"));
    //     TransformParam _transform2;
    //     _transform2.yaw = M_PI;
    //     _transform2.x = -4.7;
    //     _transform2.y = 35;
    //     _option2->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform2);
    //     _options.push_back(_option2);
    // }
    _dataManager->UseOptions(_options);

    _dataManager->addPointCloudMsgCallback(std::bind(&DemoWidget::processData, this, std::placeholders::_1));
    _dataManager->start();
}

DemoWidget::~DemoWidget()
{
    DisposeTCP();
    _dataManager->stop();
    delete _dataManager;
    delete _option;
}

void DemoWidget::InitTCP()
{
    // TCP服务器
    std::string ip = "192.168.124.65";
    unsigned short port = 5555;
    _tcpServerSender = new TcpServerSender(ip, port);
    _tcpServerSender->start();

    // 编码器
    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
    pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;
    _encoder_tcp = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionProfile, true, 0.001, 0.01, false, 30, false, 8);
}

void DemoWidget::DisposeTCP()
{
    // TCP服务器
    if (_tcpServerSender != nullptr)
    {
        _tcpServerSender->dispose();
        delete _tcpServerSender;
        _tcpServerSender = nullptr;
    }

    // 编码器
    if (_encoder_tcp != nullptr)
    {
        delete _encoder_tcp;
        _encoder_tcp = nullptr;
    }
}

void DemoWidget::setupUI()
{
    // 创建布局
    QVBoxLayout* layout = new QVBoxLayout(this);
    
    // 创建QVTKWidget
    vtkWidget = new QVTKWidget(this);
    
    // 添加到布局
    layout->addWidget(vtkWidget);
    
    // 设置主布局
    setLayout(layout);
}
#pragma endregion

#pragma region Process
bool DemoWidget::processData(PointCloudData<pcl::PointXYZI> &data)
{
    auto size = data.cloud_ptr->size();
    auto timestamp = QString::number(data.timestamp, 'f', 2);

    // std::cout << timestamp.toStdString() << "," << size << ",";

    {
        std::lock_guard<std::mutex> lock(mtx_viewer);
        // data.cloud_ptr = PointCloudHandler::GetDownSampledCloud(data.cloud_ptr, 0.05f, 0.05f, 0.05f);
        cloud_queue.push(data);
        // std::cout << data.cloud_ptr->size() << std::endl;
        if (cloud_queue.size() > 2)
            cloud_queue.pop();
    }
    return true;
}
#pragma endregion

#pragma region Update Viewer
void DemoWidget::updateViewer()
{
    if (isUpdatingDisplay)
        return;
    isUpdatingDisplay = true;

    if (cloud_queue.size() > 0)
    {
        auto start = std::chrono::high_resolution_clock::now();
        PointCloudData<pcl::PointXYZI> pointcloud;
        {
            std::lock_guard<std::mutex> lock(mtx_viewer);
            pointcloud = cloud_queue.front();
            cloud_queue.pop();
        }

        if (pointcloud.cloud_ptr->size() == 0)
        {
            isUpdatingDisplay = false;
            return;
        }

        auto timestamp = QString::number(pointcloud.timestamp, 'f', 2);
        // std::cout << timestamp.toStdString() << std::endl;

        pcl_viewer->removeAllPointClouds();
        pcl_viewer->removeAllShapes();

        // TransformParam t;
        // t.pitch = -M_PI / 300.0;
        // pointcloud.cloud_ptr = PointCloudHandler::GetTransformCloud(pointcloud.cloud_ptr, t);

        pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud2(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::copyPointCloud(*pointcloud.cloud_ptr, *pointcloud2);
        // pointcloud2 = PointCloudHandler::GetDownSampledCloud(pointcloud.cloud_ptr, 1.0, 1.0, 1.0);
        // pointcloud2 = PointCloudHandler::GetFilteredCloud(pointcloud2, 50, 1.0);
        // std::cout << "Finish Sample and Filt" << std::endl;

        // std::cout << timestamp.toStdString() << "," << pointcloud2->size() << std::endl;

        // XYZI
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> black_color(pointcloud2, 0, 0, 0);
        pcl_viewer->removePointCloud("pointcloud");
        pcl_viewer->addPointCloud<pcl::PointXYZI>(pointcloud2, black_color, "pointcloud");
        pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 1, "pointcloud");

        // XYZRGB
        // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb = getImagePointCloud(pointcloud.cloud_ptr, image);
        // pcl_viewer->removePointCloud("pointcloud");
        // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_rgb);
        // pcl_viewer->addPointCloud<pcl::PointXYZRGB>(cloud_rgb, rgb, "pointcloud");
        // pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 3, "pointcloud");

        // RangeImage
        // pcl::RangeImage::Ptr range_image = PointCloudHandler::GetDepthImage(pointcloud2);
        // pcl_viewer_range->showRangeImage(*range_image);
        // std::cout << "Finish RangeImage" << std::endl;
        //  float *ranges = range_image->getRangesArray();
        //  unsigned char *rgb_image = pcl::visualization::FloatImageUtils::getVisualImage(ranges, range_image->width, range_image->height);
        //  pcl::io::saveRgbPNGFile("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/depth2.png", rgb_image, range_image->width, range_image->height);

        // narf关键点
        // pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints = PointCloudHandler::GetNarfKeypoints(range_image);
        // pcl_viewer->removePointCloud("keypoints");
        // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler(keypoints, 0, 255, 0);
        // pcl_viewer->addPointCloud<pcl::PointXYZ>(keypoints, keypoints_color_handler, "keypoints");
        // pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");

        // 海岸线点
        //// std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> shore_points = PointCloudHandler::GetShoreLinePoints(range_image);
        // int points_id = 0;
        // std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> shore_points = PointCloudHandler::GetShoreLinePolyLineEX(range_image);
        // std::cout << "Finish GetShoreLinePoints" << std::endl;
        // for (size_t i = 0; i < shore_points.size(); i++)
        // {
        //     pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud = shore_points[i];
        //     // std::cout<<cluster_cloud->size()<<std::endl;
        //     float hue = 360.0f * i / shore_points.size(); // 色相 0-360
        //     cv::Mat3b hsv(1, 1, cv::Vec3b(hue, 255, 255));
        //     cv::Mat3b rgb;
        //     cv::cvtColor(hsv, rgb, cv::COLOR_HSV2BGR);
        //     auto color = rgb.at<cv::Vec3b>(0, 0);
        //     std::string cloud_id = "cluster_" + (points_id++);
        //     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
        //         color_handler(cluster_cloud, color[2], color[1], color[0]); // OpenCV BGR 转 RGB

        //     pcl_viewer->removePointCloud(cloud_id);
        //     pcl_viewer->addPointCloud<pcl::PointXYZ>(cluster_cloud, color_handler, cloud_id);
        //     pcl_viewer->setPointCloudRenderingProperties(
        //         PCL_VISUALIZER_POINT_SIZE, 5, cloud_id); // 设置点大小
        // }

        // 海岸线段
        // int lines_id = 0;
        // std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> shore_lines = PointCloudHandler::GetShoreLinePolyLine(range_image);
        // std::cout << "Finish GetShoreLinePolyLine" << std::endl;
        // for (size_t i = 0; i < shore_lines.size(); i++)
        // {
        //     pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud = shore_lines[i];
        //     for (size_t j = 0; j < cluster_cloud->size() - 1; j += 1)
        //     {
        //         pcl::PointXYZ p1 = cluster_cloud->points[j];
        //         pcl::PointXYZ p2 = cluster_cloud->points[j + 1];
        //         pcl_viewer->removeShape("line_" + lines_id);
        //         pcl_viewer->addLine<pcl::PointXYZ>(p1, p2, 0, 255, 0, "line_" + lines_id);
        //         pcl_viewer->setShapeRenderingProperties(PCL_VISUALIZER_LINE_WIDTH, 2, "line_" + lines_id);
        //         lines_id++;
        //     }
        // }

        // 海岸线外点
        // pcl::PointCloud<pcl::PointXYZI>::Ptr sac_points = PointCloudHandler::GetLinePlanOutSac(pointcloud.cloud_ptr, range_image);
        // pcl_viewer->removePointCloud("sac_points");
        // pcl_viewer->addPointCloud<pcl::PointXYZI>(sac_points, "sac_points");
        // pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 1, "sac_points");

        // 海岸线内点
        // pcl::PointCloud<pcl::PointXYZI>::Ptr in_points = PointCloudHandler::GetLinePlanInSac(pointcloud.cloud_ptr, range_image);
        // std::vector<pcl::PointIndices> in_clusters = PointCloudHandler::GetKdtreeCluster(in_points, 100, 100);
        // std::cout << "Finish GetKdtreeCluster" << std::endl;
        // int cluster_id = 0;
        // for (const auto &cluster_indices : in_clusters)
        // {
        //     // 提取单个聚类的点云
        //     pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        //     pcl::copyPointCloud(*in_points, cluster_indices, *cluster_cloud);

        //     // 生成HSV渐变色
        //     float hue = 360.0f * cluster_id / in_clusters.size();
        //     cv::Mat3b hsv(1, 1, cv::Vec3b(hue, 255, 255));
        //     cv::Mat3b rgb;
        //     cv::cvtColor(hsv, rgb, cv::COLOR_HSV2BGR);
        //     auto color = rgb.at<cv::Vec3b>(0, 0);

        //     // 创建颜色处理器
        //     std::string cloud_name = "in_cluster_" + std::to_string(cluster_id++);
        //     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI>
        //         color_handler(cluster_cloud, color[2], color[1], color[0]); // BGR转RGB

        //     // 更新可视化
        //     pcl_viewer->removePointCloud(cloud_name);
        //     pcl_viewer->addPointCloud<pcl::PointXYZI>(cluster_cloud, color_handler, cloud_name);
        //     pcl_viewer->setPointCloudRenderingProperties(
        //         PCL_VISUALIZER_POINT_SIZE, 3, cloud_name);
        // }

        // 法线
        //  pcl::PointCloud<pcl::Normal>::Ptr normals = PointCloudHandler::GetNormalCloud(pointcloud2, pointcloud.cloud_ptr, 10);
        //  pcl_viewer->removePointCloud("normals");
        //  pcl_viewer->addPointCloudNormals<pcl::PointXYZI, pcl::Normal>(pointcloud2, normals, 2, 200, "normals");
        //  std::cout << "Normals Num:" << normals->size() << std::endl;

        // Camera
        pcl::visualization::Camera camera_params;
        pcl_viewer->getCameraParameters(camera_params);
        std::cout << camera_params.pos[0] << "," << camera_params.pos[1] << "," << camera_params.pos[2] << std::endl;
        std::cout << camera_params.view[0] << "," << camera_params.view[1] << "," << camera_params.view[2] << std::endl;

        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
        std::cout << "耗时：" << duration.count() << " 毫秒" << std::endl;

        pcl_viewer->spinOnce();
    }
    else
    {
        isUpdatingDisplay = false;
    }

    isUpdatingDisplay = false;
}

void DemoWidget::updateSavePCD()
{
    if (cloud_queue.size() > 0)
    {
        PointCloudData<pcl::PointXYZI> pointcloud;
        {
            std::lock_guard<std::mutex> lock(mtx_viewer);
            pointcloud = cloud_queue.front();
            cloud_queue.pop();
        }

        if (pointcloud.cloud_ptr->size() <= 0)
            return;

        PointCloudHandler::SaveAsPCD("/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/" + Utils::TimeStamp2DateTimeString(pointcloud.timestamp) + ".pcd", pointcloud.cloud_ptr);
    }
}

void DemoWidget::updateSendStream()
{
    if (isSendStream)
        return;
    isSendStream = true;

    if (cloud_queue.size() > 0)
    {
        auto start = std::chrono::high_resolution_clock::now();

        // 提取出点云
        PointCloudData<pcl::PointXYZI> pointcloud;
        {
            std::lock_guard<std::mutex> lock(mtx_viewer);
            pointcloud = cloud_queue.front();
            cloud_queue.pop();
        }

        pointcloud.cloud_ptr = PointCloudHandler::GetDownSampledCloud(pointcloud.cloud_ptr, 100.0, 100.0, 100.0, true);
        //   pointcloud.cloud_ptr = PointCloudHandler::GetFilteredCloud(pointcloud.cloud_ptr, 50, 1.0);
        pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::copyPointCloud(*pointcloud.cloud_ptr, *_cloud); // 1ms

        std::stringstream _stream;
        double tempStamp = pointcloud.timestamp;
        uint64_t host_timestamp = static_cast<uint64_t>(tempStamp * 1000);
        uint64_t net_timestamp = htonll(host_timestamp);
        _stream.write(reinterpret_cast<const char *>(&net_timestamp), sizeof(uint64_t));
        _encoder_tcp->encodePointCloud(_cloud->makeShared(), _stream);

        if (_tcpServerSender != nullptr)
            _tcpServerSender->send(_stream);

        // pcl::io::savePCDFileASCII("home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/" + Utils::TimeStamp2DateTimeString(tempStamp) + ".pcd", *_cloud);

        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
        auto sendTimeStamp = (double)std::chrono::duration_cast<std::chrono::milliseconds>(end.time_since_epoch()).count() / 1000;
        std::cout << "数据帧时间戳: " << Utils::TimeStamp2DateTimeString(tempStamp) << "ms " << " 发送时间戳: " << Utils::TimeStamp2DateTimeString(sendTimeStamp) << "ms " << " 数据大小: " << _stream.str().size() << "字节 " << " 耗时：" << duration.count() << " 毫秒 " << "点云数量: " << _cloud->size() << std::endl;
    }

    isSendStream = false;
}

void DemoWidget::updateSaveStream()
{
    if (isSavedStream)
        return;
    isSavedStream = true;

    if (_encoder == nullptr)
    {
        pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
        pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;
        _encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionProfile, true, 0.001, 0.01, false, 30, false, 8);
    }

    if (cloud_queue.size() > 0)
    {
        // 提取出点云
        PointCloudData<pcl::PointXYZI> pointcloud;
        {
            std::lock_guard<std::mutex> lock(mtx_viewer);
            pointcloud = cloud_queue.front();
            cloud_queue.pop();
        }

        // 1fps编码
        double tempStamp = pointcloud.timestamp;
        if (tempStamp - lastTimeStamp >= 1)
        {
            // 记录当前帧位置
            std::streampos frame_pos = _stream.tellp();

            lastTimeStamp = tempStamp;
            encodeNum++;
            pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud(new pcl::PointCloud<pcl::PointXYZ>());
            pcl::copyPointCloud(*pointcloud.cloud_ptr, *_cloud); // 1ms

            //_stream.write(reinterpret_cast<const char *>(&net_timestamp), sizeof(uint64_t));
            _encoder->encodePointCloud(_cloud->makeShared(), _stream);

            uint64_t host_timestamp = static_cast<uint64_t>(tempStamp * 1000);
            uint64_t net_timestamp = htonll(host_timestamp);
            stream_index.emplace_back(host_timestamp, frame_pos);

            if (startTimeStamp == 0)
            {
                startTimeStamp = pointcloud.timestamp; // 记录文件起始时间
            }
        }

        if (tempStamp - startTimeStamp >= 1 * 60) // 1*60s一记录
        {
            std::string file_name = Utils::TimeStamp2DateTimeString(startTimeStamp) + "-" + Utils::TimeStamp2DateTimeString(tempStamp) + ".stream";
            std::ofstream output("/home/school/cxz/Projects/isc-lib-lidar/build/data/stream/" + file_name, std::ios::binary);

            // 临时文件头
            char header[16] = {0};
            output.write(header, 16);

            // 数据区
            output << _stream.rdbuf();

            // 索引区
            const std::streampos index_start = output.tellp();
            // 索引条数目
            uint64_t entry_count = htonll(stream_index.size());
            output.write(reinterpret_cast<const char *>(&entry_count), sizeof(uint64_t));
            // 索引条目
            for (const auto &[ts, pos] : stream_index)
            {
                uint64_t net_ts = htonll(ts);
                uint64_t net_pos = htonll(static_cast<uint64_t>(pos)); // 在数据块中的偏移量
                output.write(reinterpret_cast<const char *>(&net_ts), sizeof(uint64_t));
                output.write(reinterpret_cast<const char *>(&net_pos), sizeof(uint64_t));
            }

            // 回填文件头
            output.seekp(0);
            uint64_t net_index_start = htonll(static_cast<uint64_t>(index_start));
            output.write(reinterpret_cast<const char *>(&net_index_start), sizeof(uint64_t));
            output.write(reinterpret_cast<const char *>(&entry_count), sizeof(uint64_t));

            output.close();
            _stream.str("");

            std::cout << file_name << " , " << encodeNum << std::endl;

            encodeNum = 0;
            startTimeStamp = 0;
            lastTimeStamp = 0;
            delete _encoder;
            _encoder = nullptr;
            stream_index.clear();
        }
    }

    isSavedStream = false;
}
#pragma endregion

#pragma region IO
void DemoWidget::TestReadPCD()
{
    while (true)
    {
        std::string file = "/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/20250227171644039.pcd";
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

        auto start = std::chrono::high_resolution_clock::now();
        if (pcl::io::loadPCDFile<pcl::PointXYZI>(file, *cloud) != -1)
        {
            auto end = std::chrono::high_resolution_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
            std::cout << "耗时：" << duration.count() << " 毫秒" << std::endl;
        }
        else
        {
            std::cout << "load file " << file << " failed." << std::endl;
        }
    }
}

void DemoWidget::TestReadStream()
{
    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);

    // 时间范围
    std::string start_time_str = "20250421154700000";
    std::string end_time_str = "20250421155700000";
    double start_time_stamp = Utils::DateTimeString2TimeStamp(start_time_str);
    double end_time_stamp = Utils::DateTimeString2TimeStamp(end_time_str);
    std::vector<std::string> file_list = Utils::FindRelevantFiles("/home/school/cxz/Projects/isc-lib-lidar/build/data/files/2", start_time_stamp, end_time_stamp);

    for (int i = 0; i < file_list.size(); i++)
        std::cout << file_list[i] << std::endl;

    // 解码器
    pcl::io::OctreePointCloudCompression<pcl::PointXYZ> *_decoder = nullptr;
    std::stringstream buffer_data;
    std::stringstream buffer_index;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    double decode_time_stamp = 0;

    // 遍历时间段内文件
    int file_index = 0;
    int frame_index = 0;
    uint64_t frame_count = 0;
    if (file_index < file_list.size())
    {
        // 读取文件
        std::cout << "Load file : " << file_list[file_index] << std::endl;
        std::ifstream input(file_list[file_index], std::ios::binary);
        std::string file_content((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
        input.close();

        // 解析文件头
        const char *data = file_content.data();
        uint64_t index_start = ntohll(*reinterpret_cast<const uint64_t *>(data));
        uint64_t entry_count = ntohll(*reinterpret_cast<const uint64_t *>(data + sizeof(uint64_t)));

        // 创建独立缓冲区
        buffer_data.write(data + 16, index_start - 16);
        buffer_index.write(data + index_start, file_content.size() - index_start);
        std::cout << "数据区大小: " << buffer_data.str().size()
                  << " 索引区大小: " << buffer_index.str().size()
                  << " 原始文件大小: " << file_content.size() << std::endl;

        // 创建解码器
        pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;
        _decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionProfile, true, 0.001, 0.01, false, 30, false, 8);

        // 读取索引区
        buffer_index.read(reinterpret_cast<char *>(&frame_count), sizeof(uint64_t));
        frame_count = ntohll(frame_count);
        while (decode_time_stamp < start_time_stamp)
        {
            uint64_t net_ts, net_pos;
            buffer_index.read(reinterpret_cast<char *>(&net_ts), sizeof(uint64_t));
            buffer_index.read(reinterpret_cast<char *>(&net_pos), sizeof(uint64_t));
            decode_time_stamp = (double)ntohll(net_ts) / 1000; // 找到初始time,此时index已被读出
            buffer_data.seekg(ntohll(net_pos));                // 找到初始data，此时data还未被读出

            std::string timestamp = Utils::TimeStamp2DateTimeString(decode_time_stamp);
            std::cout << "Find First: " << timestamp << " , " << frame_index << std::endl;

            frame_index++;
        }
    }

    // 解码
    std::cout << "Data replay start." << std::endl;
    auto real_time_start = std::chrono::high_resolution_clock::now();
    while (decode_time_stamp <= end_time_stamp)
    {
        bool isUpdated = false;
        auto real_time_now = std::chrono::high_resolution_clock::now();
        auto duration = double(std::chrono::duration_cast<std::chrono::milliseconds>(real_time_now - real_time_start).count()) / 1000;

        // 解码当前时间前一段时间内所有未解码的帧
        while (decode_time_stamp <= start_time_stamp + duration)
        {
            isUpdated = true;

            // 解码帧
            _decoder->decodePointCloud(buffer_data, cloud);

            // 判断时间戳
            frame_index++;
            if (frame_index >= frame_count)
                break;
            uint64_t net_ts, net_pos;
            buffer_index.read(reinterpret_cast<char *>(&net_ts), sizeof(uint64_t));
            buffer_index.read(reinterpret_cast<char *>(&net_pos), sizeof(uint64_t));
            decode_time_stamp = (double)ntohll(net_ts) / 1000;
        }

        // 对点云做处理
        if (isUpdated)
        {
            std::string timestamp = Utils::TimeStamp2DateTimeString(decode_time_stamp);
            std::cout << "Update: " << timestamp << " , " << frame_index << " , " << frame_count << std::endl;
            pcl::PCDWriter writer;
            writer.writeBinary("/home/school/cxz/Projects/isc-lib-lidar/build/data/pcd/" + timestamp + ".pcd", *cloud);
        }

        // 判断切换文件
        if (frame_index >= frame_count)
        {
            // 清空资源
            delete _decoder;
            buffer_data.clear();
            buffer_data.str("");
            buffer_index.clear();
            buffer_index.str("");
            file_index++;
            frame_index = 0;
            if (file_index >= file_list.size())
                break;

            // 开启新文件
            std::cout << "Load file : " << file_list[file_index] << std::endl;
            std::ifstream input(file_list[file_index], std::ios::binary);
            std::string file_content((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
            input.close();

            // 解析文件头
            const char *data = file_content.data();
            uint64_t index_start = ntohll(*reinterpret_cast<const uint64_t *>(data));
            uint64_t entry_count = ntohll(*reinterpret_cast<const uint64_t *>(data + sizeof(uint64_t)));

            // 创建独立缓冲区
            buffer_data.write(data + 16, index_start - 16);
            buffer_index.write(data + index_start, file_content.size() - index_start);
            std::cout << "数据区大小: " << buffer_data.str().size()
                      << " 索引区大小: " << buffer_index.str().size()
                      << " 原始文件大小: " << file_content.size() << std::endl;

            // 读取索引区
            buffer_index.read(reinterpret_cast<char *>(&frame_count), sizeof(uint64_t));
            frame_count = ntohll(frame_count);

            // 创建解码器
            pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;
            _decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionProfile, true, 0.001, 0.01, false, 20, false, 8);

            uint64_t net_ts, net_pos;
            buffer_index.read(reinterpret_cast<char *>(&net_ts), sizeof(uint64_t));
            buffer_index.read(reinterpret_cast<char *>(&net_pos), sizeof(uint64_t));
            decode_time_stamp = (double)ntohll(net_ts) / 1000; // 找到初始time,此时index已被读出
            frame_index++;

            std::string timestamp = Utils::TimeStamp2DateTimeString(decode_time_stamp);
            std::cout << timestamp << std::endl;
            std::cout << frame_index << "," << frame_count << std::endl;
        }
    }

    std::cout << "Data replay end." << std::endl;
}
#pragma endregion

#pragma region Method
pcl::PointCloud<pcl::PointXYZRGB>::Ptr DemoWidget::getImagePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, cv::Mat &image)
{
    auto start = std::chrono::high_resolution_clock::now();

    // 相机参数
    // 外参矩阵
    Eigen::Matrix4f extrinsic_mat[6];
    Eigen::Matrix3f intrinsic_mat;
    double camera_x_offset[6] = {-2, -2, 0, 2, 0, 0};
    double camera_y_offset[6] = {-5, -4, -2.5, -5, 0, 0};
    intrinsic_mat << 5.0 / 0.00846, 0, 341.0, 0, 5.0 / 0.00846, 300, 0, 0, 1;
    for (int i = 0; i < 6; i++)
    {
        extrinsic_mat[i] = Eigen::Matrix4f::Identity();
        Eigen::Matrix3f R = (Eigen::AngleAxisf(Utils::Rad(90 + camera_y_offset[i]), Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(Utils::Rad(210 + i * 60 + camera_x_offset[i]), Eigen::Vector3f::UnitZ())).toRotationMatrix();
        Eigen::Vector3f T = Eigen::Vector3f(0, 1300, 0);
        extrinsic_mat[i].block<3, 3>(0, 0) = R;
        extrinsic_mat[i].block<3, 1>(0, 3) = T;
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::copyPointCloud(*cloud, *cloud_rgb);

    for (auto &point : *cloud_rgb)
    {
        if (point.x == 0 && point.y == 0 && point.z == 0)
            continue;

        Eigen::Vector4f world_coord(point.x, point.y, point.z, 1.0f);
        // Eigen::Vector4f world_coord(point.x * 100, point.y * 100, point.z * 100, 1.0f);
        //  计算成像镜头
        double angle = atan2(point.x, point.y);
        angle = angle < 0 ? angle + 2 * M_PI : angle;
        int index = (int)((angle + M_PI) / (M_PI / 3.0)) % 6;
        // 计算单镜头成像点
        Eigen::Vector3f image_coord = intrinsic_mat * extrinsic_mat[index].block<3, 4>(0, 0) * world_coord;
        // 映射为全景图片像素坐标
        double xRatio = image_coord.x() / image_coord.z() / 683.0, yRatio = image_coord.y() / image_coord.z() / 600.0;

        int u = (int)((xRatio / 6.0 + index * (1 / 6.0) + 0.0375) * image.cols), v = (int)(yRatio * image.rows);
        u = (u + image.cols) % image.cols;
        v = std::min(v, image.rows - 1);
        v = std::max(v, 0);

        // 赋值rgb
        cv::Vec3b color = image.at<cv::Vec3b>(v, u);
        point.r = color[2];
        point.g = color[1];
        point.b = color[0];

        cv::circle(image_write, cv::Point(u, v), 1, cv::Scalar(0, 0, 255), -1);
    }

    if (!isSavedPng)
    {
        isSavedPng = true;
        cv::imwrite("/home/school/cxz/Projects/isc-lib-lidar/build/data/image/output.png", image_write);
    }

    auto end = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
    // std::cout << "数量: " << cloud_rgb->size() << " 耗时：" << duration.count() << " 毫秒" << std::endl;

    return cloud_rgb;
}

void DemoWidget::TestImage()
{
    std::string path = "/home/school/cxz/Projects/isc-lib-lidar/build/data/opencv/";
    ImageHandler::GetShoreLinePoints(path);
}
#pragma endregion